/**
 * @file v4l2.c
 * @brief V4L2 capture device handling implementation with robust error handling.
 */

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <stdlib.h>

#include "v4l2.h"

static int v4l2_check_capability(v4l2_context_t *ctx)
{
    struct v4l2_capability caps;
    memset(&caps, 0, sizeof caps);

    int ret = ioctl(ctx->fd, VIDIOC_QUERYCAP, &caps);
    RETURN_ON_FAIL(ret < 0, "VIDIOC_QUERYCAP failed: %s\n", ERRSTR);

    ctx->is_multiplanar = caps.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE;
    RETURN_ON_FAIL(!ctx->is_multiplanar, "V4L2: Multi-planar capture is not supported\n");

    printf("V4L2 Driver: %s, Card: %s\n", caps.driver, caps.card);
    return 0;
}

static int v4l2_set_format(v4l2_context_t *ctx)
{
    struct v4l2_format fmt;
    memset(&fmt, 0, sizeof(struct v4l2_format));

    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    fmt.fmt.pix_mp.width = ctx->width;
    fmt.fmt.pix_mp.height = ctx->height;
    fmt.fmt.pix_mp.pixelformat = ctx->in_fourcc;
    fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;

    int ret = ioctl(ctx->fd, VIDIOC_S_FMT, &fmt);
    RETURN_ON_FAIL(ret < 0, "VIDIOC_S_FMT failed: %s\n", ERRSTR);

    memset(&fmt, 0, sizeof(struct v4l2_format));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    ret = ioctl(ctx->fd, VIDIOC_G_FMT, &fmt);
    RETURN_ON_FAIL(ret < 0, "VIDIOC_G_FMT failed: %s\n", ERRSTR);

    ctx->in_fourcc = fmt.fmt.pix_mp.pixelformat;
    ctx->width = fmt.fmt.pix_mp.width;
    ctx->height = fmt.fmt.pix_mp.height;
    ctx->num_planes = fmt.fmt.pix_mp.num_planes;

    ctx->plane_pitches = calloc(ctx->num_planes, sizeof(unsigned int));
    ctx->plane_offsets = calloc(ctx->num_planes, sizeof(unsigned int));
    RETURN_ON_FAIL(!ctx->plane_pitches || !ctx->plane_offsets, "Failed to allocate plane info memory\n");

    for (unsigned int i = 0; i < ctx->num_planes; ++i)
    {
        ctx->plane_pitches[i] = fmt.fmt.pix_mp.plane_fmt[i].bytesperline;
    }

    printf("V4L2 format set: %dx%d, 4cc=%.4s, planes=%u, pitch[0]=%u\n",
           ctx->width, ctx->height, (char *)&ctx->in_fourcc, ctx->num_planes, ctx->plane_pitches[0]);
    return 0;
}

static int v4l2_request_buffers(v4l2_context_t *ctx)
{
    struct v4l2_requestbuffers rqbufs;
    memset(&rqbufs, 0, sizeof(struct v4l2_requestbuffers));

    rqbufs.count = ctx->buffer_count;
    rqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    rqbufs.memory = V4L2_MEMORY_DMABUF;

    int ret = ioctl(ctx->fd, VIDIOC_REQBUFS, &rqbufs);
    RETURN_ON_FAIL(ret < 0, "VIDIOC_REQBUFS failed: %s\n", ERRSTR);
    RETURN_ON_FAIL(rqbufs.count < ctx->buffer_count, "Allocated only %u of %u buffers\n", rqbufs.count, ctx->buffer_count);

    ctx->buffer_count = rqbufs.count;
    printf("V4L2 buffer count set to %d.\n", ctx->buffer_count);
    return 0;
}

int v4l2_init(v4l2_context_t *ctx)
{
    ctx->fd = open(ctx->device, O_RDWR);
    RETURN_ON_FAIL(ctx->fd < 0, "failed to open %s: %s\n", ctx->device, ERRSTR);

    if (v4l2_check_capability(ctx) < 0)
        goto fail;
    if (v4l2_set_format(ctx) < 0)
        goto fail;
    if (v4l2_request_buffers(ctx) < 0)
        goto fail;

    return 0;

fail:
    close(ctx->fd);
    ctx->fd = -1;
    return -1;
}

void v4l2_cleanup(v4l2_context_t *ctx)
{
    if (ctx->fd >= 0)
    {
        close(ctx->fd);
        ctx->fd = -1;
    }
    free(ctx->plane_pitches);
    free(ctx->plane_offsets);
    ctx->plane_pitches = NULL;
    ctx->plane_offsets = NULL;
}

int v4l2_stream_on(v4l2_context_t *ctx, struct drm_buffer *drm_bufs)
{
    for (unsigned int i = 0; i < ctx->buffer_count; ++i)
    {
        struct v4l2_buffer buf;
        struct v4l2_plane planes[VIDEO_MAX_PLANES];
        memset(&buf, 0, sizeof(buf));
        memset(planes, 0, sizeof(planes));

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        buf.memory = V4L2_MEMORY_DMABUF;
        buf.index = i;
        buf.m.planes = planes;
        buf.length = ctx->num_planes;

        for (unsigned int p = 0; p < ctx->num_planes; ++p)
        {
            buf.m.planes[p].m.fd = drm_bufs[i].dma_fd;
        }

        int ret = ioctl(ctx->fd, VIDIOC_QBUF, &buf);
        RETURN_ON_FAIL(ret < 0, "VIDIOC_QBUF for buffer %d failed: %s\n", i, ERRSTR);
    }

    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    int ret = ioctl(ctx->fd, VIDIOC_STREAMON, &type);
    RETURN_ON_FAIL(ret < 0, "VIDIOC_STREAMON failed: %s\n", ERRSTR);

    printf("V4L2 streaming started.\n");
    return 0;
}

int v4l2_stream_off(v4l2_context_t *ctx)
{
    if (ctx->fd < 0)
        return 0;
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    int ret = ioctl(ctx->fd, VIDIOC_STREAMOFF, &type);
    if (ret < 0)
    {
        fprintf(stderr, "WARN: VIDIOC_STREAMOFF failed: %s\n", ERRSTR);
        return -1;
    }
    printf("V4L2 streaming stopped.\n");
    return 0;
}

int v4l2_dequeue_buffer(v4l2_context_t *ctx, struct v4l2_buffer *buf)
{
    buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    buf->memory = V4L2_MEMORY_DMABUF;
    int ret = ioctl(ctx->fd, VIDIOC_DQBUF, buf);
    if (ret < 0)
    {
        fprintf(stderr, "VIDIOC_DQBUF failed: %s\n", ERRSTR);
    }
    return ret;
}

int v4l2_queue_buffer(v4l2_context_t *ctx, struct v4l2_buffer *buf)
{
    int ret = ioctl(ctx->fd, VIDIOC_QBUF, buf);
    if (ret < 0)
    {
        fprintf(stderr, "VIDIOC_QBUF failed for buffer %d: %s\n", buf->index, ERRSTR);
    }
    return ret;
}
